/**
  ******************************************************************************
  * File Name          : ext_sns.cpp
  * Description       : ext_sns program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <ext_sns_driver/ext_sns.h>

std::string _port;
serial::Serial ser;

unsigned char rxd[RXD_MAX]; 
unsigned char txd[TXD_MAX];  

std::string id_s[IR_MAX] = {"1","2","3","4"};  

float temp;
float hum;         

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "ext_sns_driver");
    ros::NodeHandle nh;
    ros::Rate loop_rate(20);

    uint16_t data,data1;

    sensor_msgs::Temperature  _temp;
    sensor_msgs::RelativeHumidity _hum;
    sensor_msgs::Range _range[IR_MAX];
    ros::Publisher ir_pub[IR_MAX];
    ros::Publisher temp_pub;
    ros::Publisher hum_pub;

    uint8_t i;
    
    nh.getParam("/ext_sns_driver/port", _port);

    temp_pub = nh.advertise<sensor_msgs::Temperature>("/temperature", 5);
    hum_pub = nh.advertise<sensor_msgs::RelativeHumidity>("/humidity", 5);

    for(i=0; i<IR_MAX; i++)
    {
        ir_pub[i] = nh.advertise<sensor_msgs::Range>("/ir_sense/"+id_s[i]+"/range", 5);
    }

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort(_port.c_str()); 
        ser.setBaudrate(9600); 
        serial::Timeout to = serial::Timeout::simpleTimeout(5); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 

    while (ros::ok())
    {
        txd[0] = 1;
        ser.write(txd,TXD_MAX);
        if (ser.available())
        {
            ser.read(rxd, RXD_MAX);
/*
            printf("rxd:[");
            for(int i=0; i<RXD_Msensor_msgs/TemperatureAX; i++)
            {

                printf(" %02x "_range[i],rxd[i]);
            }
            printf("]\r\n");
*/
            if((rxd[0]==0x5a)&&(rxd[1]==0)&&(rxd[10]==0xff))
            {
                data = rxd[2] << 8 | rxd[3];
                data1 = rxd[4] << 8 | rxd[5];
                temp = ((float)data*165/65535)-40.0;
                hum = ((float)data1/65535)*100;

                _temp.header.stamp = ros::Time::now();
                _temp.temperature = temp;

                _hum.header.stamp = ros::Time::now();
                _hum.relative_humidity = hum;

                _range[0].range  = rxd[6];
                _range[1].range  = rxd[7];
                _range[2].range  = rxd[8];
                _range[3].range  = rxd[9];
                //printf("temp: %f hum: %f\r\n", temp, hum);
            }
            
            temp_pub.publish(_temp);
            hum_pub.publish(_hum);
            for(i=0; i<IR_MAX; i++)
            {
                ir_pub[i].publish(_range[i]);
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

    if(ser.isOpen())
    {
        ser.close();
    }
  
    return 0;
}
